#include "State_JumpL.h"
#include "Dog.h"

//#define ANGLE_F RAD(70)//缩腿站立
//#define ANGLE_B RAD(80)
#define preANGLE2_F RAD(30)//在缩腿站立状态下需继续旋转的角度
#define preANGLE2_B RAD(30)
#define POINTANGLE1 RAD(100)

#define KP0 9.5f
#define KD0 1.3f
#define KP1 9.5f
#define KD1 1.3f
#define KP2 8.0f
#define KD2 1.3f
#define KP3 8.0f
#define KD3 1.3f

void JumpL_Enter(void)
{
    UART_Print("JumpL enter\n");
}

void JumpL_Run(void)
{
    /*曲腿准备*/
    /*HT_SetPosition(dog.legs[0].motorF, ANGLE_F-preANGLE2_F, KP0, KD0, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F-preANGLE2_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F-preANGLE2_F, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F-preANGLE2_F, KP3, KD3, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B+preANGLE2_B, KP0, KD0, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B+preANGLE2_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B+preANGLE2_B, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B+preANGLE2_B, KP3, KD3, 0);*/

    /*旋钮控制*/
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F-global.jumpAngleR, KP0, KD0, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F-global.jumpAngleL, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F-global.jumpAngleL, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F-global.jumpAngleR, KP3, KD3, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B+global.jumpAngleR, KP0, KD0, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B+global.jumpAngleL, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B+global.jumpAngleL, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B+global.jumpAngleR, KP3, KD3, 0);
}

void JumpL_Exit(void)
{
    UART_Print("JumpL exit\n");
}



